#include <Wire.h>
#include <LiquidCrystal.h>
#include <SoftwareSerial.h>
#include "CommDb.h"
#include "Mp3Table.h"

//------------------------------------------------ App Define
#define sbi(port, bit) (port) |= (1 << (bit))
#define cbi(port, bit) (port) &= ~(1 << (bit))

#define Open 0
#define Close 1

//------------------------------------------------ Pin Define
#define pin_Rxd      0  //D0, Rxd
#define pin_Txd      1  //D1, Txd
#define pin_MOT_DirR  2  //D2, INT0
#define pin_MOT_PwmR  3  //D3, PWM3(TC2), INT1
#define pin_MOT_DirL  4  //D4
#define pin_MOT_PwmL  5  //D5, PWM5(TC0)
#define pin_LCD_RS    6  //D6, PWM6(TC0)
#define pin_LCD_EN    7  //D7
#define pin_LCD_D4    8  //D8
#define pin_LCD_D5    9  //D9, PWM9(TC1)
#define pin_LCD_D6    10  //D10, PWM10(TC1)
#define pin_LCD_D7    11  //D11, PWM11(TC2), MOSI
#define pin_PushBtn   12  //D12, MISO
#define pin_LED     13  //D13, SCLK, LED

//Analog pin
#define pin_BtRxd     14  //A0
#define pin_BtTxd      15  //A1
#define pin_Piezo      16  //A2
#define pin_Lever      17  //A3
#define pin_SCK        18  //A4, SCK
#define pin_SCL        19  //A5, SCL



//------------------------------------------------ Device Define
#define HCSR04_Qty 8

#define CmdWheel_STOP() analogWrite(pin_MOT_PwmL, 0);\
                                            analogWrite(pin_MOT_PwmR, 0)

#define CmdLeftWheel_FWD(PWM) digitalWrite(pin_MOT_DirL, LOW);\
                                                        analogWrite(pin_MOT_PwmL, PWM)

#define CmdLeftWheel_REV(PWM) digitalWrite(pin_MOT_DirL, HIGH);\
                                                        analogWrite(pin_MOT_PwmL, PWM)

#define CmdLeftWheel_STOP() analogWrite(pin_MOT_PwmL, 0)

#define CmdRightWheel_FWD(PWM)  digitalWrite(pin_MOT_DirR, LOW);\
                                                          analogWrite(pin_MOT_PwmR, PWM)

#define CmdRightWheel_REV(PWM)  digitalWrite(pin_MOT_DirR, HIGH);\
                                                         analogWrite(pin_MOT_PwmR, PWM)
                                            
#define CmdRightWheel_STOP()  analogWrite(pin_MOT_PwmR, 0)


//------------------------------------------------ Distance Sensor
#define DSensId_FL  0
#define DSensId_FR  1
#define DSensId_RF 2
#define DSensId_RB 3
#define DSensId_BR  4 
#define DSensId_BL  5
#define DSensId_LB 6
#define DSensId_LF 7

#define DistanceThreshold_Red_mm 200
#define DistanceThreshold_Blue_mm 500

//------------------------------------------------  Global Variable Define
LiquidCrystal lcd(pin_LCD_RS, pin_LCD_EN, pin_LCD_D4, pin_LCD_D5, pin_LCD_D6, pin_LCD_D7);

SoftwareSerial Bluetooth(pin_BtRxd, pin_BtTxd);


int BluetoothTxd[3];
struct sJoystickCmd
{
  int JoyX, JoyY;
  byte Btn_654321;
}JoystickCmd;

tDistance_mm Distance_mm;
tEncoder Encoder;

struct sFindDistance{
  byte Ch;
  unsigned short Distance_mm;
};

struct
{
  byte Mode :1;
  byte ActiveStop :1;
}MotorPositionControlFlag;

byte OperatingMode=0;
#define OprMode_Guide 1
#define OprMode_Remote 2
#define OprMode_Avoid  3
#define OprMode_Trace  4
#define OprMode_Mp3Player 5
#define OprMode_Test   6
#define OprMode_UltraSensMon 7
#define OprMode_EtcSensMon  8
#define MaxOprMode 8

//------------------------------------------------ 
void setup()
{
  Wire.begin(); // join i2c bus (address optional for master)
  Serial.begin(115200);  
  Bluetooth.begin(57600);
  //Bluetooth.begin(115200);
  lcd.begin(16, 2);
  //lcd.blink();

  pinMode(pin_PushBtn, INPUT_PULLUP);
  pinMode(pin_Piezo, OUTPUT);
  pinMode(pin_LED, OUTPUT);  
  pinMode(pin_MOT_DirL, OUTPUT);  
  pinMode(pin_MOT_PwmL, OUTPUT);  
  pinMode(pin_MOT_DirR, OUTPUT);  
  pinMode(pin_MOT_PwmR, OUTPUT);  
  
  CmdWheel_STOP();
  Init_JoystickBluetoothComm();
//  Put_EncoderSetCounter(0, 0);

  MotorPositionControlFlag.Mode=Open;
  MotorPositionControlFlag.ActiveStop=true;

  Serial.println("Start DongDong-2");
  PlayPiezo(1);

  delay(1000); //Wait for Wake up all Arduino 
/*
  Play_Mp3_UntilStop(SND_H_Donghaya);
  Play_Mp3_UntilStop(SND_H_Hello);
  Play_Mp3_UntilStop(SND_H_Nanundongdongyiya);
*/  
  //Waiting Here ........................
  OperatingMode=SelectOprMode();
  Serial.print("Operating Mode:");
  Serial.print(OperatingMode);
  Serial.println("");
}

void loop()
{
  static long TimeStamp[2]={0,};
  long CurTime;  
 
  switch(OperatingMode){
  case OprMode_Test:
    Proc_Test();
    break;
  case OprMode_Remote:
    Proc_BluetoothRx();
    JoystickDriveCmd(JoystickCmd.JoyX, JoystickCmd.JoyY);  
    break;
  case OprMode_Avoid:
    Proc_I2C();
    Proc_OprMode_Avoid();
    break;
  case OprMode_Trace:
    Proc_I2C();
    Proc_OprMode_Trace();
    break;
  case OprMode_UltraSensMon:
    Proc_I2C();
    Proc_OprMode_UltraSensMon();
    break;
  case OprMode_EtcSensMon:
    Proc_I2C();
    Proc_OprMode_EtcSensMon();
    break;
  case OprMode_Guide:
    Proc_I2C();
    Proc_OprMode_Guide();
    break;
  case OprMode_Mp3Player:
    Proc_OprMode_Mp3Player();
    break;
  default:
    break;      
  }   


  switch(OperatingMode){
  //case OprMode_Test:
  case OprMode_UltraSensMon:
  case OprMode_EtcSensMon:
  case OprMode_Mp3Player:  
  //case OprMode_Remote:
  //case OprMode_Avoid:
  //case OprMode_Trace:
  //case OprMode_Guide:
    break;
  default:
    //BGM Play when every 30sec
    CurTime=millis();
    if((CurTime-TimeStamp[1]) > (1000*30)){
      TimeStamp[1]=CurTime;
      if(Get_Mp3BusyStatus() == false){
        static byte MusicNo=70;
        MusicNo++;
        if(MusicNo > 74){
          MusicNo=70;
        }
        //Play_Mp3_WhenNew(MusicNo);  
        Play_Mp3_Intercept(MusicNo);  
      }
    }  
    break;      
  }   

  CurTime=millis();
  if((CurTime-TimeStamp[0]) > 200){
    TimeStamp[0]=CurTime;
    ToggleLed();

    if(OperatingMode==OprMode_Remote){
      Proc_BluetoothTx();
    }
    
/*
    #if 0

    #else
      Serial.print((unsigned short)Get_HeadPosion_deg_F32());
    #endif
    Serial.println("");
*/    

  #define DisplayMode 0

  #if DisplayMode == 1
    for(idx=0; idx<HCSR04_Qty; idx++){      
      Serial.print(EchoWidth_us[idx]);Serial.print("us, ");
    }
    Serial.println("");
  #elif DisplayMode == 2
    for(idx=0; idx<HCSR04_Qty; idx++){      
      Serial.print(Distance_mm.Ch[idx]);Serial.print("mm, ");
    }
    for(idx=0; idx<2; idx++){      
      Serial.print(Encoder.Counter[idx]);Serial.print(", ");
    }
    Serial.println("");
  #elif DisplayMode == 3
    for(idx=0; idx<HCSR04_Qty; idx++){      
      //if(Distance_mm.Ch[Idx] < 1500){
      if(Distance_mm.Ch[Idx] < 300){
        Serial.print("1");
      }
      else{
        Serial.print("0");
      }
    }
  #elif DisplayMode == 4
    Serial.print(JoystickCmd.JoyX);
    Serial.print(", ");
    Serial.print(JoystickCmd.JoyY);
    Serial.print(", ");
    Serial.print(JoystickCmd.Btn_654321, BIN); 
    Serial.println("");
  #else
  #endif
  }
}
/*
void Turn_CenterPivot(int Rotate_deg)
void Drive(long Distance_cm)
*/

void Proc_Test(void)
{
  static long TimeStamp=0;
  long CurTime;
  static bool ActivationStatus=false;
  static long CheckTime_ms=0;
  
  Proc_I2C();

  CurTime=millis();  
  if((CurTime-TimeStamp) > CheckTime_ms){
    TimeStamp=CurTime;
    if(ActivationStatus==false){
      ActivationStatus=true;
      CheckTime_ms=(long)30*1000; //On Time
    }
    else{
      ActivationStatus=false;
      //CheckTime_ms=(long)2*60*1000; //Off Time
      CheckTime_ms=(long)50*1000; //Off Time
    }
  }
  
  if(ActivationStatus==true){
    //lcd.setCursor(0, 1); lcd.print("T               ");
    Guide_Drive();
  }
  else{
    //lcd.setCursor(0, 1); lcd.print("F               ");
    ManualMotorCmd(0, 0);
  }
}

void Proc_OprMode_Mp3Player(void)
{
  if(Get_Mp3BusyStatus() == false){
    static byte MusicNo=100;
    Play_Mp3_Intercept(MusicNo);  
    MusicNo++;
    if(MusicNo > 130){
      MusicNo=100;
    }
  }
}

#define LeverPush 0
#define LeverNeutral  1
#define LeverPull 2
byte GetLeverStatus(void)
{
  byte LeverCmd;
 
  if(analogRead(pin_Lever) > 550){
    LeverCmd=LeverPush;
  }
  else{
    LeverCmd=LeverNeutral;
  }
  
  return LeverCmd;
}


void Guide_Drive(void)
{
  if(Encoder.IrSens > 0x50){ //Not Covered
    //Distance of Front a nd Hight is not enough
    if(Distance_mm.Ch[DSensId_LF] < Distance_mm.Ch[DSensId_RF]){
      Play_Mp3_WhenNew(SND_H_Right);
      Turn_CenterPivot(180, Close, 40);
    }
    else{
      Play_Mp3_WhenNew(SND_H_Left);
      Turn_CenterPivot(-180, Close, 40);
    }
  }
//  else if(((Distance_mm.Ch[DSensId_FL] > 350) && (Distance_mm.Ch[DSensId_FR] > 350))){
    else if(((Distance_mm.Ch[DSensId_FL] > 500) && (Distance_mm.Ch[DSensId_FR] > 500))){
    if(Distance_mm.Ch[DSensId_LF] < 200){
      Play_Mp3_WhenNew(SND_H_Right);
      ManualMotorCmd(50, 0);
    }
    else if(Distance_mm.Ch[DSensId_RF] < 200){
      Play_Mp3_WhenNew(SND_H_Left);
      ManualMotorCmd(0, 50);
    }
    else{
      Play_Mp3_WhenNew(SND_H_Fwd);
      ManualMotorCmd(90, 90);
    }
  }
  else{
    if(Distance_mm.Ch[DSensId_LF] < Distance_mm.Ch[DSensId_RF]){
      Play_Mp3_WhenNew(SND_H_Right);
      //Turn_CenterPivot(180, Close, 40);
      Turn_CenterPivot(90, Close, 40);
    }
    else{
      Play_Mp3_WhenNew(SND_H_Left);
      //Turn_CenterPivot(-180, Close, 40);
      Turn_CenterPivot(-90, Close, 40);      
    }
  }
}

void Proc_OprMode_Guide(void)
{
  if(GetLeverStatus()==LeverPush){
    //Play_Mp3_Intercept(SND_H_Start);
    Guide_Drive();
  }
  else{
    Play_Mp3_WhenNew(SND_H_Stop);
    ManualMotorCmd(0, 0);
  }
}

void Proc_OprMode_Trace(void)
{
  struct sFindDistance MinDistance;
  FindMinDistance(&MinDistance);    
  while(MinDistance.Distance_mm > DistanceThreshold_Red_mm){
    switch(MinDistance.Ch){
    case DSensId_FL:
    case DSensId_FR:
      //Detect Foward
      Drive(40, Open, 100);
      break;
    case DSensId_RF:
    case DSensId_RB:
      //Detect Right
      Turn_CenterPivot(45, Open, 100);
      break;
    case DSensId_BL:
    case DSensId_BR:
      //Detect Backward
      Drive(-40, Open, 100);
      break;
    case DSensId_LF:
    case DSensId_LB:
      //Detect Left  
      Turn_CenterPivot(-45, Open, 100);
      break;  
    default:
      break; 
    }
    Read_I2C(I2C_SlaveAddr_SRHC04, (unsigned char *)&Distance_mm, sizeof(Distance_mm));
    FindMinDistance(&MinDistance); 
  }
  ManualMotorCmd(0, 0);
}

void Proc_OprMode_Avoid(void)
{
  struct sFindDistance MinDistance;
  FindMinDistance(&MinDistance);  
  if(MinDistance.Distance_mm < DistanceThreshold_Blue_mm){
    PlayPiezo(2);
    while((Distance_mm.Ch[MinDistance.Ch] <= (DistanceThreshold_Blue_mm+20))){
      switch(MinDistance.Ch){
      case DSensId_FL:
      case DSensId_FR:
        //Detect Foward
        if((Distance_mm.Ch[DSensId_BR] > DistanceThreshold_Red_mm) && (Distance_mm.Ch[DSensId_BL] > DistanceThreshold_Red_mm)){
          ManualMotorCmd(-100, -100);
        }
        else{
          ManualMotorCmd(0, 0);
        }
        break;
      case DSensId_RF:
      case DSensId_RB:
        //Detect Right
        if((Distance_mm.Ch[DSensId_LF] > DistanceThreshold_Red_mm) && (Distance_mm.Ch[DSensId_LB] > DistanceThreshold_Red_mm)){
          ManualMotorCmd(-100, 100);
        }
        else{
          ManualMotorCmd(0, 0);
        }        
        break;
      case DSensId_BR:
      case DSensId_BL:
        //Detect Backward
        if((Distance_mm.Ch[DSensId_FR] > DistanceThreshold_Red_mm) && (Distance_mm.Ch[DSensId_FL] > DistanceThreshold_Red_mm)){
          ManualMotorCmd(100, 100);
        }
        else{
          ManualMotorCmd(0, 0);
        }        
        break;
      case DSensId_LF:
      case DSensId_LB:
        //Detect Left
        if((Distance_mm.Ch[DSensId_RF] > DistanceThreshold_Red_mm) && (Distance_mm.Ch[DSensId_RB] > DistanceThreshold_Red_mm)){
          ManualMotorCmd(100, -100);
        }
        else{
          ManualMotorCmd(0, 0);
        }        
        break;  
      default:
        break; 
      }
      Read_I2C(I2C_SlaveAddr_SRHC04, (unsigned char *)&Distance_mm, sizeof(Distance_mm));
      FindMinDistance(&MinDistance);  
      Put_Mp3_Init();
    }
    
    if(Get_Mp3BusyStatus()==false){
      Play_Mp3_WhenNew(SND_H_Najapahba);
    }
    ManualMotorCmd(0, 0);
  }
}

void ManualMotorCmd(char LeftMotorDuty_percent, char RightMotorDuty_percent)
{
  if(LeftMotorDuty_percent < 0){
    CmdLeftWheel_REV(Percent2U8(abs(LeftMotorDuty_percent)));
  }
  else{
    CmdLeftWheel_FWD(Percent2U8(abs(LeftMotorDuty_percent)));
  }

  if(RightMotorDuty_percent < 0){
    CmdRightWheel_REV(Percent2U8(abs(RightMotorDuty_percent)));
  }
  else{
    CmdRightWheel_FWD(Percent2U8(abs(RightMotorDuty_percent)));
  }

}


void Proc_OprMode_UltraSensMon(void)
{
  static long TimeStamp=0;
  long CurTime;  
  
  CurTime=millis();
  if((CurTime-TimeStamp) > 200){
    TimeStamp=CurTime;  
    lcd.setCursor(0, 0); lcd.print("                ");
    lcd.setCursor(0, 0); lcd.print(Distance_mm.Ch[DSensId_LF]/10);lcd.print(",");
    lcd.setCursor(4, 0); lcd.print(Distance_mm.Ch[DSensId_FL]/10);lcd.print(",");
    lcd.setCursor(8, 0); lcd.print(Distance_mm.Ch[DSensId_FR]/10);lcd.print(",");
    lcd.setCursor(12, 0); lcd.print(Distance_mm.Ch[DSensId_RF]/10);
    lcd.setCursor(0, 1); lcd.print("                ");
    lcd.setCursor(0, 1); lcd.print(Distance_mm.Ch[DSensId_LB]/10);lcd.print(",");
    lcd.setCursor(4, 1); lcd.print(Distance_mm.Ch[DSensId_BL]/10);lcd.print(",");
    lcd.setCursor(8, 1); lcd.print(Distance_mm.Ch[DSensId_BR]/10);lcd.print(",");
    lcd.setCursor(12, 1); lcd.print(Distance_mm.Ch[DSensId_RB]/10);
  }
}

void Proc_OprMode_EtcSensMon(void)
{
  static long TimeStamp=0;
  long CurTime;  
  
  CurTime=millis();
  if((CurTime-TimeStamp) > 200){
    TimeStamp=CurTime;  
    lcd.setCursor(0, 0); lcd.print("                ");
    lcd.setCursor(0, 0); lcd.print("EnL:");lcd.print((char)Encoder.Counter[1], DEC);
    lcd.setCursor(8, 0); lcd.print("EnR:");lcd.print((char)Encoder.Counter[0], DEC);
    lcd.setCursor(0, 1); lcd.print("                ");
    lcd.setCursor(0, 1); lcd.print("Lev:");lcd.print(analogRead(pin_Lever), HEX);
    lcd.setCursor(8, 1); lcd.print("Ir:");lcd.print(Encoder.IrSens, HEX);
  }
}



byte SelectOprMode(void)
{
  byte OperatingMode=0;
  bool pastBtnStatus=0;
  bool curBtnStatus;  
  unsigned char BtnLowStateCount=0;
  
  lcd.setCursor(0, 0); lcd.print("DongDong-2 Mode");
   
  while(1){
    curBtnStatus=digitalRead(pin_PushBtn);
    if((pastBtnStatus==1) && (curBtnStatus == 0)){
      PlayPiezo(1);
      BtnLowStateCount=0;
    }
    else if((pastBtnStatus==0) && (curBtnStatus == 1)){
      OperatingMode++;
      if(OperatingMode > MaxOprMode){
        OperatingMode=1;  
      }
      lcd.setCursor(0, 1); lcd.print("                ");
      switch(OperatingMode){
      case OprMode_Remote:
        lcd.setCursor(0, 1); lcd.print("Remote");
        Play_Mp3_Intercept(SND_H_RemoteMode);
        break;
      case OprMode_Avoid:
        lcd.setCursor(0, 1); lcd.print("Avoid");
        Play_Mp3_Intercept(SND_H_AvoidMode);
        break;
      case OprMode_Trace:
        lcd.setCursor(0, 1); lcd.print("Trace");
        Play_Mp3_Intercept(SND_H_TraceMode);
        break;
      case OprMode_Guide:
        lcd.setCursor(0, 1); lcd.print("Walking Guide");
        Play_Mp3_Intercept(SND_H_GuideMode);
        break;
      case OprMode_Mp3Player:
        lcd.setCursor(0, 1); lcd.print("MP3 Player");
        Play_Mp3_Intercept(SND_H_Mp3Mode);
        break;  
      case OprMode_Test:
        lcd.setCursor(0, 1); lcd.print("Test");
        Play_Mp3_Intercept(SND_H_TestMode);
        break;         
      case OprMode_UltraSensMon:
        lcd.setCursor(0, 1); lcd.print("UltraSens Mon");
        Play_Mp3_Intercept(SND_H_DiagMode);
        break;   
      case OprMode_EtcSensMon:
        lcd.setCursor(0, 1); lcd.print("EtcSens Mon");
        Play_Mp3_Intercept(SND_H_DiagMode);
        break;         
      default:
        lcd.setCursor(0, 1); lcd.print("Not Define");
        break;      
      }     
      lcd.blink();
    }
    else if((pastBtnStatus==0) && (curBtnStatus == 0)){
      if(BtnLowStateCount > 100){
        PlayPiezo(3);
        lcd.noBlink();    
        break;
      }
      BtnLowStateCount++;
    }
    pastBtnStatus=curBtnStatus;
    delay(10);
//    Proc_I2C();
  }
  
  return OperatingMode;
}





//#define OneTurnEncoderPulse_CenterPivot 4561 //Proto
#define OneTurnEncoderPulse_CenterPivot 4952 //Final
void Turn_CenterPivot(long Rotate_deg, bool ControlMode, byte MaxPwmLimit_percent)
{
  float Rotate_pls;
  Rotate_pls=OneTurnEncoderPulse_CenterPivot*(float)Rotate_deg/360;
  Rotate_pls=abs(Rotate_pls);
 
  MotorPositionControlFlag.Mode=ControlMode; //PutMotorPositionControlLoop(ControlMode);
  if(Rotate_deg > 0){
    MotorPositionController(-(long)Rotate_pls, (long)Rotate_pls, MaxPwmLimit_percent);
  }
  else{
    MotorPositionController((long)Rotate_pls, -(long)Rotate_pls, MaxPwmLimit_percent);
  }
}

#define OneCmDriveEncoderPulse 57
void Drive(long Distance_cm, bool ControlMode, byte MaxPwmLimit_percent)
{
  long Distance_pls;
  Distance_pls=OneCmDriveEncoderPulse*Distance_cm;
  MotorPositionControlFlag.Mode=ControlMode; //PutMotorPositionControlLoop(ControlMode);
  MotorPositionController(Distance_pls, Distance_pls, MaxPwmLimit_percent);
}

/*
  struct sFindDistance MinDistance;
  FindMinDistance(&MinDistance);  
  if(MinDistance.Distance_mm < DistanceThreshold_Blue_mm){
*/

bool IsVeryClose(long TargetR, long TargetL)
{
  struct sFindDistance MinDistance;
  Read_I2C(I2C_SlaveAddr_SRHC04, (unsigned char *)&Distance_mm, sizeof(Distance_mm));
  FindMinDistance(&MinDistance);  
  if(MinDistance.Distance_mm < DistanceThreshold_Red_mm){
    if(((TargetL > 0) && (TargetR > 0)) && ((MinDistance.Ch == DSensId_FR) || (MinDistance.Ch == DSensId_FL))){
      //Foward Detect
      return true;  
    }
    else if(((TargetL < 0) && (TargetR < 0)) && ((MinDistance.Ch == DSensId_BR) || (MinDistance.Ch == DSensId_BL))){
      //Backward Detect
      return true;  
    }
    else if(((TargetL > 0) && (TargetR <= 0)) && ((MinDistance.Ch == DSensId_RF) || (MinDistance.Ch == DSensId_RB))){
      //Right Turn Detect
      return true;  
    }
    else if(((TargetL <= 0) && (TargetR > 0)) && ((MinDistance.Ch == DSensId_LF) || (MinDistance.Ch == DSensId_LB))){
      //Left Turn Detect
      return true;  
    }
    else{
      return false;
    } 
  }
  else{
    return false;
  }
}



/*
bool GetMotorPositionControlLoop(void)
{
  return MotorPositionControlFlag.Mode;
}

void PutMotorPositionControlLoop(bool Mode)
{
  MotorPositionControlFlag.Mode=Mode;
}
*/


#define P_Gain 0.7
#define I_Gain 0.007F
#define Threshold_AdaptI_Gain 100
#define ErrorTolerance 1
#define MinPWM  70
#define VerifyLoopCnt 100
void MotorPositionController(long TargetR_pls, long TargetL_pls, byte MaxPwmLimit_percent)
{
  long ErrorR, ErrorL;
  long IntegErrorR=0, IntegErrorL=0;
  float PWM_CommandR, PWM_CommandL;
  word VerifyCnt=VerifyLoopCnt;
  bool DetectedZeroErr_R=false;
  bool DetectedZeroErr_L=false;
  byte MaxPwmLimit;

  Put_EncoderSetCounter(0, 0);
 
  MaxPwmLimit=Percent2U8(MaxPwmLimit_percent);
    
  while(VerifyCnt--){
    do{
      ErrorR=TargetR_pls-Encoder.Counter[0];
      if(abs(ErrorR) < Threshold_AdaptI_Gain){
        IntegErrorR=IntegErrorR+ErrorR;
      }
      else{
        IntegErrorR=0;
      }
      PWM_CommandR=(ErrorR*P_Gain) + (IntegErrorR*I_Gain);
      PWM_CommandR=abs(PWM_CommandR) + MinPWM;
      if(PWM_CommandR > MaxPwmLimit){
        PWM_CommandR=MaxPwmLimit;
      }      
      if(MotorPositionControlFlag.Mode==Open){
        if((abs(ErrorR) <= ErrorTolerance) || (DetectedZeroErr_R == true)){
          DetectedZeroErr_R = true;
          PWM_CommandR=0;      
        }
      }
      if(ErrorR < 0){
        CmdRightWheel_REV((unsigned char)PWM_CommandR);
      }
      else{
        CmdRightWheel_FWD((unsigned char)PWM_CommandR);
      } 
      
 
      ErrorL=TargetL_pls-Encoder.Counter[1];
      if(abs(ErrorL) < Threshold_AdaptI_Gain){
        IntegErrorL=IntegErrorL+ErrorL;
      }
      else{
        IntegErrorL=0;
      }
      PWM_CommandL=(ErrorL*P_Gain) + (IntegErrorL*I_Gain);
      PWM_CommandL=abs(PWM_CommandL) + MinPWM;
      if(PWM_CommandL > MaxPwmLimit){
        PWM_CommandL=MaxPwmLimit;
      }  
      if(MotorPositionControlFlag.Mode==Open){
        if((abs(ErrorL) <= ErrorTolerance) || (DetectedZeroErr_L == true)){
          DetectedZeroErr_L = true;
          PWM_CommandL=0;      
        }
      }
      if(ErrorL < 0){
        CmdLeftWheel_REV((unsigned char)PWM_CommandL);
      }
      else{
        CmdLeftWheel_FWD((unsigned char)PWM_CommandL);
      }

      if(MotorPositionControlFlag.ActiveStop==true){
        if(IsVeryClose(TargetR_pls, TargetL_pls)==true){  
          CmdWheel_STOP();       
          return;
        }
      }
      if(OperatingMode == OprMode_Guide){
        if(GetLeverStatus() != LeverPush){
          CmdWheel_STOP();       
          return;
        }
      }
      if((DetectedZeroErr_R == true) && (DetectedZeroErr_L == true)){
        CmdWheel_STOP();       
        return;
      }

      Read_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)&Encoder, sizeof(Encoder));
    }while((abs(TargetR_pls - Encoder.Counter[0]) > ErrorTolerance) || (abs(TargetL_pls - Encoder.Counter[1]) > ErrorTolerance));
    IntegErrorR=0;
    IntegErrorL=0;
  }
  
  CmdWheel_STOP();  
  Put_EncoderSetCounter(0, 0);
}



void FindMinDistance(struct sFindDistance *pFindDistance)
{
  byte ChNum;
  pFindDistance->Distance_mm=-1;
  for(ChNum=0; ChNum<(sizeof(Distance_mm.Ch)/sizeof(Distance_mm.Ch[0])); ChNum++){
    if(pFindDistance->Distance_mm > Distance_mm.Ch[ChNum]){
        pFindDistance->Ch=ChNum;
        pFindDistance->Distance_mm=Distance_mm.Ch[ChNum];
    }
  }
}

void FindMaxDistance(struct sFindDistance *pFindDistance)
{
  byte ChNum;
  pFindDistance->Distance_mm=0;
  for(ChNum=0; ChNum<(sizeof(Distance_mm.Ch)/sizeof(Distance_mm.Ch[0])); ChNum++){
    if(pFindDistance->Distance_mm < Distance_mm.Ch[ChNum]){
        pFindDistance->Ch=ChNum;
        pFindDistance->Distance_mm=Distance_mm.Ch[ChNum];
    }
  }
}


unsigned char Percent2U8(unsigned char Percent)
{
  if(Percent>=100){
    return 0xff;
  }
  else if(Percent==0){
    return 0;
  }
  else{
    return (unsigned char)((float)0.01F*Percent*255);
  }
}

void JoystickDriveCmd(int Cmd_JoyX, int Cmd_JoyY)
{
  byte Scalar_X, Dir_X;
  byte Scalar_Y, Dir_Y;
  byte Scalar_XY;
  
  byte PWM_L=0, PWM_R=0; 

  if(Cmd_JoyX >= 0){
    Scalar_X=(byte)Cmd_JoyX;
    Dir_X='+';
  }  
  else{
    Scalar_X=(byte)-1*Cmd_JoyX;
    Dir_X='-';
  }

  if(Cmd_JoyY >= 0){
    Scalar_Y=(byte)Cmd_JoyY;
    Dir_Y='+';
  }  
  else{
    Scalar_Y=(byte)-1*Cmd_JoyY;
    Dir_Y='-';
  }
 
  Scalar_XY=(byte)sqrt(Scalar_X*Scalar_X+Scalar_Y*Scalar_Y);
  if(Scalar_XY > 100){
    Scalar_XY=100;
  }

//  Serial.print(", X=");Serial.write(Dir_X);Serial.print(Scalar_X);
//  Serial.print(", Y=");Serial.write(Dir_Y);Serial.print(Scalar_Y);
//  Serial.print(", XY=");Serial.print(Scalar_XY);
 
  if(Scalar_XY==0){
    CmdWheel_STOP();
  }
  else if(Cmd_JoyY > 0){
    PWM_L=Scalar_XY;
    PWM_R=Scalar_XY;
 
    if(Scalar_X > 20){        
      if(Dir_X == '+'){
        PWM_R=PWM_R-Scalar_X;
      }
      else{
        PWM_L=PWM_L-Scalar_X;
      }
    }
//    Serial.print(", PWM_L=");Serial.print(PWM_L);
//    Serial.print(", PWM_R=");Serial.print(PWM_R);    
    CmdLeftWheel_FWD(Percent2U8(PWM_L));
    CmdRightWheel_FWD(Percent2U8(PWM_R));
  }
  else if(Cmd_JoyY < 0){
    if(Scalar_X > 30){
      if(Dir_X == '+'){
//        Serial.print(", Pivot-R");
          CmdLeftWheel_FWD(Percent2U8(Scalar_XY));
          CmdRightWheel_REV(Percent2U8(Scalar_XY));
      }
      else{
//        Serial.print(", Pivot-L");
          CmdLeftWheel_REV(Percent2U8(Scalar_XY));
          CmdRightWheel_FWD(Percent2U8(Scalar_XY));
      }
    }
    else{
//      Serial.print(", Back:");Serial.print(Scalar_XY);
        CmdLeftWheel_REV(Percent2U8(Scalar_XY));
        CmdRightWheel_REV(Percent2U8(Scalar_XY ));
    }
  } 
}



////////////////////////////////////////////////////////////////////////////////// I2C Driver
void Proc_I2C(void)
{
  byte Idx;
  
//  tDistance_mm Distance_mm;
  byte curMode_NeoPixel;
  tBarGraph BarGraph;
  tChar Char;
  tString String;
  tPixel Pixel;
  
/*
  Char.Param=TestVal;
  Put_NeoPixel(Cmd_NeoPixel_Char, (unsigned char *)&Char, sizeof(Char));
  
  memcpy(&String, "HelloWorld", 10);
  String.Param[0]=TestVal+10;
  Put_NeoPixel(Cmd_NeoPixel_String, (unsigned char *)&String, sizeof(String));
*/

//  Read_I2C(I2C_SlaveAddr_NeoPixel, (unsigned char *)&curMode_NeoPixel, sizeof(curMode_NeoPixel));

  Read_I2C(I2C_SlaveAddr_SRHC04, (unsigned char *)&Distance_mm, sizeof(Distance_mm));
  Read_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)&Encoder, sizeof(Encoder));

  memset(&Pixel, 0, sizeof(Pixel));
  Make_Pixel(&Pixel);
  Put_NeoPixel(Cmd_NeoPixel_Pixel, (unsigned char *)&Pixel, sizeof(Pixel));

/*
  #define Distance_mm_Limit  500
  for(Idx=0; Idx<8; Idx++){      
    if(Distance_mm.Ch[Idx] > Distance_mm_Limit){
      Distance_mm.Ch[Idx]=Distance_mm_Limit;
    }
    BarGraph.Ch[Idx]=Distance_mm.Ch[Idx]/(Distance_mm_Limit/5);
  }
  Put_NeoPixel(Cmd_NeoPixel_BarGraph, (unsigned char *)&BarGraph, sizeof(BarGraph));
*/
}

void Make_Pixel(tPixel *pPixel)
{
  byte SensorIdx;
  word SensorData;
  byte Color;
  
  for(SensorIdx=0; SensorIdx<8; SensorIdx++){
    SensorData=Distance_mm.Ch[SensorIdx];
    if(SensorData < DistanceThreshold_Red_mm){
      Color=Pxl_RED;
    }
    else if(SensorData < DistanceThreshold_Blue_mm){
      Color=Pxl_BLUE;
    }
    else{
      Color=Pxl_GREEN;
    }
    
    switch(SensorIdx){
    case 0:
      pPixel->X[0].Y5=Color;
      break;
    case 1:
      pPixel->X[0].Y2=Color;
      break;
    case 2:
      pPixel->X[1].Y0=Color;
      break;
    case 3:
      pPixel->X[3].Y0=Color;
      break;
    case 4:
      pPixel->X[4].Y2=Color;
      break;
    case 5:
      pPixel->X[4].Y5=Color;
      break;
    case 6:
      pPixel->X[3].Y7=Color;
      break;
    case 7:
      pPixel->X[1].Y7=Color;
      break;
    default:
      break;  
    }
  }
}


void Write_I2C(unsigned char DeviceAddr, unsigned char *pSrcAddr, unsigned char Len)
{
  Wire.beginTransmission(DeviceAddr);
  Wire.write(pSrcAddr, Len);
  Wire.endTransmission();
}

void Read_I2C(unsigned char DeviceAddr, unsigned char *pDstAddr, unsigned char Len)
{
  Wire.requestFrom(DeviceAddr, Len); 
  while(Len){
//    if(Wire.available()){
      *pDstAddr=Wire.read();
      pDstAddr++;
      Len--;
//    }
  }
}

void Put_NeoPixel(unsigned char Cmd, unsigned char *pData, unsigned char DataLen)
{
  unsigned char TxBuf[DataLen+1];
  TxBuf[0]=Cmd;
  memcpy(&TxBuf[1], pData, DataLen);
  Write_I2C(I2C_SlaveAddr_NeoPixel, (unsigned char *)TxBuf, sizeof(TxBuf));
}

byte prevCmd=0xff;
byte prevData=0xff;
void Put_Mp3(byte Cmd, byte Data)
{
  byte TxBuf[3];
  
  if((prevCmd != Cmd) || (prevData != Data)){
    TxBuf[0]=Cmd_Encoder_Mp3Cmd;
    TxBuf[1]=Cmd;
    TxBuf[2]=Data;
    Write_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)TxBuf, sizeof(TxBuf));
  }
  prevCmd = Cmd;
  prevData = Data;
}

void Put_Mp3_Init(void)
{
 prevCmd=0xff;
}

void Put_Mp3_Raw(byte Cmd, byte Data)
{
  byte TxBuf[3];
  
  TxBuf[0]=Cmd_Encoder_Mp3Cmd;
  TxBuf[1]=Cmd;
  TxBuf[2]=Data;
  Write_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)TxBuf, sizeof(TxBuf));
}


void Play_Mp3_WhenNew(byte SongNo)
{
  Put_Mp3(Mp3_Play, SongNo);
}

void Play_Mp3_Intercept(byte SongNo)
{
  Put_Mp3_Raw(Mp3_Play, SongNo);
}

void Play_Mp3_UntilStop(byte SongNo)
{
  Put_Mp3_Raw(Mp3_Play, SongNo);
  while(Get_Mp3BusyStatus() == true);
}

byte Get_Mp3BusyStatus(void)
{
  delay(200);
  Read_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)&Encoder, sizeof(Encoder));  
  return Encoder.Mp3Status;
}

void Put_EncoderSetCounter(long EncoderR, long EncoderL)
{
  unsigned char TxBuf[sizeof(tEncoderSetCounter)+1];
  tEncoderSetCounter EncoderSetCounter;
  EncoderSetCounter.Data[0]=EncoderR;
  EncoderSetCounter.Data[1]=EncoderL;
  
  TxBuf[0]=Cmd_Encoder_SetCounter;
  memcpy(&TxBuf[1], &EncoderSetCounter, sizeof(tEncoderSetCounter));

/*
  Write_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)TxBuf, sizeof(TxBuf));
  do{
    Read_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)&Encoder, sizeof(Encoder));  
  }while((Encoder.Counter[0]!=EncoderR) || (Encoder.Counter[1]!=EncoderL));
*/  
  
  Write_I2C(I2C_SlaveAddr_Encoder, (unsigned char *)TxBuf, sizeof(TxBuf));
}

////////////////////////////////////////////////////////////////////////////////// Comm Drivers
void Init_JoystickBluetoothComm(void)
{
  memset(&JoystickCmd, 0, sizeof(JoystickCmd));
  memset(BluetoothTxd, 0, sizeof(BluetoothTxd));
}

void Proc_RxKeyword(byte Cmd)
{
//  Serial.print("Keyword = 0x");Serial.println(Cmd, HEX);
}

void Proc_RxJoystickCmd(byte *pJoyCmdPkt)
{
  int X, Y;
  X= (pJoyCmdPkt[0]-'0')*100 + (pJoyCmdPkt[1]-'0')*10 + (pJoyCmdPkt[2]-'0') - 200;
  Y= (pJoyCmdPkt[3]-'0')*100 + (pJoyCmdPkt[4]-'0')*10 + (pJoyCmdPkt[5]-'0') - 200;  
  if((-100<=X) && (X<=100) && (-100<=Y) && (Y<=100)){
//    Serial.print("Joystick X=");Serial.print(X);Serial.print(", Y=");Serial.print(Y);
    JoystickCmd.JoyX=X;
    JoystickCmd.JoyY=Y;
//    CalcDriveCmd(X, Y);
//    Serial.println("");
  }
  else{
//    Serial.println("Error !!! : undefined Joystick command");
  }
}

void Proc_RxButtonCmd(byte Cmd)
{
  switch(Cmd){
//------------------ Button-1
  case 'A':
    sbi(JoystickCmd.Btn_654321, 0);
//    Serial.println("BTN1-On");
    break;
  case 'B':
    cbi(JoystickCmd.Btn_654321, 0);
//    Serial.println("BTN1-Off");
    break;
//------------------ Button-2
  case 'C':
    sbi(JoystickCmd.Btn_654321, 1);
//    Serial.println("BTN2-On");
    break;
  case 'D':
    cbi(JoystickCmd.Btn_654321, 1);
//    Serial.println("BTN2-Off");
    break;
//------------------ Button-3    
  case 'E':
    sbi(JoystickCmd.Btn_654321, 2);
//    Serial.println("BTN3-On");
    break;
  case 'F':
    cbi(JoystickCmd.Btn_654321, 2);
//    Serial.println("BTN3-Off");
    break;
//------------------ Button-4        
  case 'G':
    sbi(JoystickCmd.Btn_654321, 3);
//    Serial.println("BTN4-On");
    break;
  case 'H':
    cbi(JoystickCmd.Btn_654321, 3);
//    Serial.println("BTN4-Off");
    break;
//------------------ Button-5        
  case 'I':
    sbi(JoystickCmd.Btn_654321, 4);
//    Serial.println("BTN5-On");
    break;
  case 'J':
    cbi(JoystickCmd.Btn_654321, 4);
//    Serial.println("BTN5-Off");
    break;
//------------------ Button-6        
  case 'K':
    sbi(JoystickCmd.Btn_654321, 5);
//    Serial.println("BTN6-On");
    break;
  case 'L':
    cbi(JoystickCmd.Btn_654321, 5);
//    Serial.println("BTN6-Off");
    break;
//------------------ 
  default:
//    Serial.println("Error !!! : undefined button command");
    break;
  }
}

#define    STX          0x02
#define    ETX          0x03
#define Len_BtnCmdPkt 3    
#define Len_JoyCmdPkt 8
void Proc_BluetoothRx()
{
  static byte RxPkt[7]={0, 0, 0, 0, 0, 0, 0};
  static byte Idx=0;
  byte Rxd;
  byte CompleteRxPkt;
  
  while (Bluetooth.available()) {
    Rxd = Bluetooth.read();
    if(Rxd >= 0x80){
      //Receive char about Keyword Protocol
      Proc_RxKeyword(Rxd);
    }
    else{
      //Receive char about Joystick_BT_Command Protocol
      //Check for complete of packet
      CompleteRxPkt=0;
      if(Rxd==ETX){
        if(Idx==(Len_BtnCmdPkt-1)){
            //if(('A' <= RxPkt[1]) && (RxPkt[1] <= 'L')){
            if(RxPkt[1] >= 'A'){
              //BtnCmd
              CompleteRxPkt=Len_BtnCmdPkt;
            }
        }
        else if(Idx==(Len_JoyCmdPkt-1)){
            if(RxPkt[1] <= '9'){
            //if(('0' <= RxPkt[1]) && (RxPkt[1] <= '9') && ('0' <= RxPkt[2]) && (RxPkt[2] <= '9')  && ('0' <= RxPkt[3]) && (RxPkt[3] <= '9')
            //  && ('0' <= RxPkt[4]) && (RxPkt[4] <= '9') && ('0' <= RxPkt[5]) && (RxPkt[5] <= '9')  && ('0' <= RxPkt[6]) && (RxPkt[6] <= '9')){
              //JoyCmd
              CompleteRxPkt=Len_JoyCmdPkt;            
            }
        }
      }

      //Packet Assembly
      if(CompleteRxPkt == 0){
        if(Idx >= sizeof(RxPkt)){
          Idx=0;      
        }

        if(!((Idx == 0) && (Rxd != STX))){
          RxPkt[Idx]=Rxd;
          Idx++;
        }

      }
      else{
        //Command Packet Rx Complete
        Idx=0;  
        if(CompleteRxPkt==Len_BtnCmdPkt){
          //Call BtnCmd Service
          Proc_RxButtonCmd(RxPkt[1]);
        }
        else if(CompleteRxPkt==Len_JoyCmdPkt){
          //Call JoyCmd Service
          Proc_RxJoystickCmd(&RxPkt[1]);
        }
      }
    }
  }
}

String getButtonStatusString(void)
{
  String bStatus = "";
  for(int i=0; i<6; i++)  {
    if(JoystickCmd.Btn_654321 & (B100000 >>i))      bStatus += "1";
    else                                              bStatus += "0";
  }
  return bStatus;
}

void Proc_BluetoothTx(void)
{
                                          Bluetooth.print((char)STX);   // Start of Transmission
    Bluetooth.print(getButtonStatusString());                 // 6-buttons status feedback
                                          Bluetooth.print((char)0x1);   
    Bluetooth.print(BluetoothTxd[0]);      
                                          Bluetooth.print((char)0x4);   
    Bluetooth.print(BluetoothTxd[1]);      
                                          Bluetooth.print((char)0x5);   
    Bluetooth.print(1);                             
                                          Bluetooth.print((char)ETX);   // End of Transmission
}

//OUT---------------------------------------------------------------------------- LED
void ToggleLed(void)
{
  static byte Cnt=0;
  digitalWrite(pin_LED, Cnt&1);
  Cnt++;
}

//OUT---------------------------------------------------------------------------- Piezo
void PlayPiezo(byte Step)
{
  word Play_Hz;
  
  Play_Hz=350;
  while(Step){
    tone(pin_Piezo, Play_Hz);    
    delay(100);  
    Play_Hz+=300;
    Step--;
  }

  noTone(pin_Piezo);    
}

